import gnu.io.NoSuchPortException;
import gnu.io.PortInUseException;

import java.io.IOException;

import javaclient3.PlayerClient;
import javaclient3.PlayerException;
import javaclient3.Position2DInterface;
import javaclient3.RangerInterface;
import javaclient3.SonarInterface;
import javaclient3.structures.PlayerConstants;

import com.googlecode.grtframework.rpc.USBRPC;
import com.googlecode.grtframework.sensor.RPCAccelerometer;
import com.googlecode.gunncs.actuator.RobotBase;
import com.googlecode.gunncs.controller.TiltController;

public class MainRobot {

	public static void main(String[] args) {
		USBRPC conn = null;
		try {
			conn = new USBRPC("/dev/ttyACM0");
		} catch (NoSuchPortException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (PortInUseException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

		RPCAccelerometer r = new RPCAccelerometer(conn,
				new int[] { 66, 67, 68 });
		r.startListening();

		PlayerClient client = null;
		Position2DInterface positioner = null;
		RangerInterface ranger = null;
		SonarInterface s = null;

		try {
			// Connect to the Player server and request access to Position and
			// Sonar
			client = new PlayerClient("localhost", 6665);
			positioner = client.requestInterfacePosition2D(0,
					PlayerConstants.PLAYER_OPEN_MODE);
			ranger = client.requestInterfaceRanger(0,
					PlayerConstants.PLAYER_OPEN_MODE);
			// s = client.requestInterfaceSonar(0,
			// PlayerConstants.PLAYER_OPEN_MODE);
		} catch (PlayerException e) {
			System.err
					.println("WallFollowerExample: > Error connecting to Player: ");
			System.err.println("    [ " + e.toString() + " ]");
			System.exit(1);
		}

		client.runThreaded(-1, -1);

		// positioner.setSpeed(0, .5);

		// RangeFinder rf = new RangeFinder(ranger);
		// rf.start();

		RobotBase b = new RobotBase(positioner);

		TiltController t = new TiltController(b, r, "tilt");
		t.initiate();

		// while (true) {
		// // s.queryGeometry();
		// // if (s.isGeomReady()) {
		// // System.out.println("yay!");
		// // }
		// try {
		// Thread.sleep(100);
		// } catch (InterruptedException e) {
		// // TODO Auto-generated catch block
		// e.printStackTrace();
		// }
		// }

	}

}
